

#include <string.h>
#include <stdint.h>

#include "octospi.h"
#include "w25q128jv.h"

int OSPI_W25Q128JV_AutoPollingMemReady(uint32_t timeout)
{
  OSPI_RegularCmdTypeDef sCmd;
  OSPI_AutoPollingTypeDef sCfg;

  memset(&sCmd, 0, sizeof(sCmd));
  memset(&sCfg, 0, sizeof(sCfg));

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_STATUS_REG1;

  sCmd.AddressMode = HAL_OSPI_ADDRESS_1_LINE;
  sCmd.AddressSize = HAL_OSPI_ADDRESS_24_BITS;
  sCmd.Address = 0;

  sCmd.DataMode = HAL_OSPI_DATA_1_LINE;
  sCmd.DummyCycles = 0;
  sCmd.NbData = 1;
  
  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  sCfg.Match = 0;
  sCfg.Mask = 0x1;
  sCfg.MatchMode = HAL_OSPI_MATCH_MODE_AND;
  sCfg.Interval = 0x10;
  sCfg.AutomaticStop = HAL_OSPI_AUTOMATIC_STOP_ENABLE;

  if (HAL_OSPI_AutoPolling(&hospi1, &sCfg, timeout) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  return OSPI_OK;
}

int OSPI_W25Q128JV_WriteEnable(void)
{
  OSPI_RegularCmdTypeDef sCmd;
  OSPI_AutoPollingTypeDef sCfg;

  memset(&sCmd, 0, sizeof(sCmd));

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_WRITE_ENABLE;
  
  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  memset(&sCmd, 0, sizeof(sCmd));
  memset(&sCfg, 0, sizeof(sCfg));

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_STATUS_REG1;

  sCmd.DataMode = HAL_OSPI_DATA_1_LINE;
  sCmd.DummyCycles = 0;
  sCmd.NbData = 1;
  
  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  sCfg.Match = 0x2;
  sCfg.Mask = 0x2;
  sCfg.MatchMode = HAL_OSPI_MATCH_MODE_AND;
  sCfg.Interval = 0x10;
  sCfg.AutomaticStop = HAL_OSPI_AUTOMATIC_STOP_ENABLE;

  if (HAL_OSPI_AutoPolling(&hospi1, &sCfg, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  return OSPI_OK;
}

int OSPI_W25Q128JV_ReadReg(uint8_t reg, uint8_t *v)
{
  OSPI_RegularCmdTypeDef sCmd;

  memset(&sCmd, 0, sizeof(sCmd));

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = reg;

  sCmd.AddressMode = HAL_OSPI_ADDRESS_1_LINE;
  sCmd.AddressSize = HAL_OSPI_ADDRESS_24_BITS;
  sCmd.Address = 0;

  sCmd.DataMode = HAL_OSPI_DATA_1_LINE;
  sCmd.DummyCycles = 0;
  sCmd.NbData = 1;
  
  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  if (HAL_OSPI_Receive(&hospi1, (uint8_t *)v, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  return OSPI_OK;
}

int OSPI_W25Q128JV_DeviceID(uint8_t *v)
{
  OSPI_RegularCmdTypeDef sCmd;
  memset(&sCmd, 0, sizeof(sCmd));

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_DEVICE_ID;

  sCmd.AddressMode = HAL_OSPI_ADDRESS_1_LINE;
  sCmd.AddressSize = HAL_OSPI_ADDRESS_24_BITS;
  sCmd.Address = 0;

  sCmd.DataMode = HAL_OSPI_DATA_1_LINE;
  sCmd.DummyCycles = 0;
  sCmd.NbData = 2;

  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  if (HAL_OSPI_Receive(&hospi1, (uint8_t *)v, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  return OSPI_OK;
}

int OSPI_W25Q128JV_IDNumber(uint8_t *v)
{
  OSPI_RegularCmdTypeDef sCmd;
  memset(&sCmd, 0, sizeof(sCmd));

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_ID_NUMBER;

  sCmd.AddressMode = HAL_OSPI_ADDRESS_1_LINE;
  sCmd.AddressSize = HAL_OSPI_ADDRESS_24_BITS;
  sCmd.Address = 0;

  sCmd.DataMode = HAL_OSPI_DATA_1_LINE;
  sCmd.DummyCycles = 0;
  sCmd.NbData = 12;

  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  if (HAL_OSPI_Receive(&hospi1, (uint8_t *)v, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  return OSPI_OK;
}

int OSPI_W25Q128JV_Reset(void)
{
  OSPI_RegularCmdTypeDef sCmd;
  memset(&sCmd, 0, sizeof(sCmd));

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_ENABLE_RESET;

  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_RESET_DEVICE;

  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  return OSPI_OK;
}

int OSPI_W25Q128JV_Read(uint8_t *pData, uint32_t ReadAddr, uint32_t Size)
{
  OSPI_RegularCmdTypeDef sCmd;
  memset(&sCmd, 0, sizeof(sCmd));

  //if (OSPI_W25Q128JV_AutoPollingMemReady(HAL_MAX_DELAY) != OSPI_OK)
  //{
  //  return OSPI_ERROR;
  //}
#if 1
  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_INPUT_FAST_READ;

  sCmd.AddressMode = HAL_OSPI_ADDRESS_4_LINES;
  sCmd.AddressSize = HAL_OSPI_ADDRESS_24_BITS;
  sCmd.Address = ReadAddr;

  //sCmd.AlternateBytesMode = HAL_OSPI_ALTERNATE_BYTES_4_LINES;
  //sCmd.AlternateBytesSize = HAL_OSPI_ALTERNATE_BYTES_8_BITS;
  //sCmd.AlternateBytes = 0;

  sCmd.DataMode = HAL_OSPI_DATA_4_LINES;
  sCmd.DummyCycles = 6;
  sCmd.NbData = Size;

#else
  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_FAST_READ_QUAD_OUTPUT;

  sCmd.AddressMode = HAL_OSPI_ADDRESS_1_LINE;
  sCmd.AddressSize = HAL_OSPI_ADDRESS_24_BITS;
  sCmd.Address = ReadAddr;

  sCmd.DataMode = HAL_OSPI_DATA_4_LINES;
  sCmd.DummyCycles = 8;
  sCmd.NbData = Size;
#endif 
  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  if (HAL_OSPI_Receive(&hospi1, pData, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  if (OSPI_W25Q128JV_AutoPollingMemReady(HAL_MAX_DELAY) != OSPI_OK)
  {
    return OSPI_ERROR;
  }

  return OSPI_OK;
}

int OSPI_W25Q128JV_EraseSector(uint32_t SectorAddress)
{
  OSPI_RegularCmdTypeDef sCmd;

  memset(&sCmd, 0, sizeof(sCmd));

  if (OSPI_W25Q128JV_WriteEnable() != OSPI_OK)
  {
    return OSPI_ERROR;
  }

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_ERASE_SECTOR;

  sCmd.AddressMode = HAL_OSPI_ADDRESS_1_LINE;
  sCmd.AddressSize = HAL_OSPI_ADDRESS_24_BITS;
  sCmd.Address = SectorAddress;

  //sCmd.AlternateBytesMode = HAL_OSPI_ALTERNATE_BYTES_4_LINES;
  //sCmd.AlternateBytesSize = HAL_OSPI_ALTERNATE_BYTES_8_BITS;
  //sCmd.AlternateBytes = 1;
  
  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  if (OSPI_W25Q128JV_AutoPollingMemReady(HAL_MAX_DELAY) != OSPI_OK)
  {
    return OSPI_ERROR;
  }

  return OSPI_OK;
}

int OSPI_W25Q128JV_ERASE_Block32K(uint32_t SectorAddress)
{
  OSPI_RegularCmdTypeDef sCmd;

  memset(&sCmd, 0, sizeof(sCmd));

  if (OSPI_W25Q128JV_WriteEnable() != OSPI_OK)
  {
    return OSPI_ERROR;
  }

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_ERASE_Block32K;

  sCmd.AddressMode = HAL_OSPI_ADDRESS_1_LINE;
  sCmd.AddressSize = HAL_OSPI_ADDRESS_24_BITS;
  sCmd.Address = SectorAddress;

  //sCmd.AlternateBytesMode = HAL_OSPI_ALTERNATE_BYTES_4_LINES;
  //sCmd.AlternateBytesSize = HAL_OSPI_ALTERNATE_BYTES_8_BITS;
  //sCmd.AlternateBytes = 1;
  
  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  if (OSPI_W25Q128JV_AutoPollingMemReady(HAL_MAX_DELAY) != OSPI_OK)
  {
    return OSPI_ERROR;
  }

  return OSPI_OK;
}

int OSPI_W25Q128JV_ERASE_Block64K(uint32_t SectorAddress)
{
  OSPI_RegularCmdTypeDef sCmd;

  memset(&sCmd, 0, sizeof(sCmd));

  if (OSPI_W25Q128JV_WriteEnable() != OSPI_OK)
  {
    return OSPI_ERROR;
  }

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_ERASE_Block64K;

  sCmd.AddressMode = HAL_OSPI_ADDRESS_1_LINE;
  sCmd.AddressSize = HAL_OSPI_ADDRESS_24_BITS;
  sCmd.Address = SectorAddress;

  //sCmd.AlternateBytesMode = HAL_OSPI_ALTERNATE_BYTES_4_LINES;
  //sCmd.AlternateBytesSize = HAL_OSPI_ALTERNATE_BYTES_8_BITS;
  //sCmd.AlternateBytes = 1;
  
  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  if (OSPI_W25Q128JV_AutoPollingMemReady(HAL_MAX_DELAY) != OSPI_OK)
  {
    return OSPI_ERROR;
  }

  return OSPI_OK;
}

int OSPI_W25Q128JV_CHIP_ERASE(void)
{
  OSPI_RegularCmdTypeDef sCmd;

  memset(&sCmd, 0, sizeof(sCmd));

  if (OSPI_W25Q128JV_WriteEnable() != OSPI_OK)
  {
    return OSPI_ERROR;
  }

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_CHIP_ERASE;

  //sCmd.AddressMode = HAL_OSPI_ADDRESS_1_LINE;
  //sCmd.AddressSize = HAL_OSPI_ADDRESS_24_BITS;
  //sCmd.Address = SectorAddress;

  //sCmd.AlternateBytesMode = HAL_OSPI_ALTERNATE_BYTES_4_LINES;
  //sCmd.AlternateBytesSize = HAL_OSPI_ALTERNATE_BYTES_8_BITS;
  //sCmd.AlternateBytes = 1;
  
  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  if (OSPI_W25Q128JV_AutoPollingMemReady(HAL_MAX_DELAY) != OSPI_OK)
  {
    return OSPI_ERROR;
  }

  return OSPI_OK;
}

int OSPI_W25Q128JV_PageProgram(uint8_t *pData, uint32_t ReadAddr, uint32_t Size)
{
  OSPI_RegularCmdTypeDef sCmd;

  memset(&sCmd, 0, sizeof(sCmd));

  if (OSPI_W25Q128JV_WriteEnable() != OSPI_OK)
  {
    return OSPI_ERROR;
  }

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_PAGE_PROGRAM;

  sCmd.AddressMode = HAL_OSPI_ADDRESS_1_LINE;
  sCmd.AddressSize = HAL_OSPI_ADDRESS_24_BITS;
  sCmd.Address = ReadAddr;

  //sCmd.AlternateBytesMode = HAL_OSPI_ALTERNATE_BYTES_4_LINES;
  //sCmd.AlternateBytesSize = HAL_OSPI_ALTERNATE_BYTES_8_BITS;
  //sCmd.AlternateBytes = 1;

  sCmd.DataMode = HAL_OSPI_DATA_1_LINE;
  sCmd.DummyCycles = 0;
  sCmd.NbData = Size;

  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  if (HAL_OSPI_Transmit(&hospi1, (uint8_t *)pData, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  if (OSPI_W25Q128JV_AutoPollingMemReady(HAL_MAX_DELAY) != OSPI_OK)
  {
    return OSPI_ERROR;
  }

  return OSPI_OK;
}

int OSPI_W25Q128JV_Write(uint8_t *pData, uint32_t WriteAddr, uint32_t Size)
{
    int ret = OSPI_OK;
    uint32_t end_addr, current_size, current_addr;
    uint8_t *write_data;

    /* Calculation of the size between the write address and the end of the page */
    current_size = W25Q128JV_PAGE_SIZE - (WriteAddr % W25Q128JV_PAGE_SIZE);

    /* Check if the size of the data is less than the remaining place in the page */
    if (current_size > Size)
    {
      current_size = Size;
    }

    /* Initialize the address variables */
    current_addr = WriteAddr;
    end_addr = WriteAddr + Size;
    write_data = pData;

    /* Perform the write page by page */
    do
    {
      /* Issue page program command */
      if (OSPI_W25Q128JV_PageProgram(write_data, current_addr, current_size) != OSPI_OK)
      {
          ret = OSPI_ERROR;
      }
      else
      {
        /* Update the address and size variables for next page programming */
        current_addr += current_size;
        write_data += current_size;
        current_size = ((current_addr + W25Q128JV_PAGE_SIZE) > end_addr) ? (end_addr - current_addr) : W25Q128JV_PAGE_SIZE;
      }
    } while ((current_addr < end_addr) && (ret == OSPI_OK));

    /* Return BSP status */
    return ret;
}

int OSPI_W25Q128JV_EnableMemoryMappedMode(void)
{
  OSPI_RegularCmdTypeDef sCmd;
  OSPI_MemoryMappedTypeDef sMapCfg;

  memset(&sCmd, 0, sizeof(sCmd));
  memset(&sMapCfg, 0, sizeof(sMapCfg));

  if (OSPI_W25Q128JV_AutoPollingMemReady(HAL_MAX_DELAY) != OSPI_OK)
  {
    return OSPI_ERROR;
  }

  sCmd.InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE;
  sCmd.InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS;
  sCmd.Instruction = W25Q128JV_INPUT_FAST_READ;

  sCmd.AddressMode = HAL_OSPI_ADDRESS_4_LINES;
  sCmd.AddressSize = HAL_OSPI_ADDRESS_24_BITS;
  sCmd.Address = 0;

  //sCmd.AlternateBytesMode = HAL_OSPI_ALTERNATE_BYTES_4_LINES;
  //sCmd.AlternateBytesSize = HAL_OSPI_ALTERNATE_BYTES_8_BITS;
  //sCmd.AlternateBytes = 1;

  sCmd.DataMode = HAL_OSPI_DATA_4_LINES;
  sCmd.DummyCycles = 6;
  sCmd.NbData = 0;

  if (HAL_OSPI_Command(&hospi1, &sCmd, HAL_MAX_DELAY) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  sMapCfg.TimeOutActivation = HAL_OSPI_TIMEOUT_COUNTER_DISABLE;

  if (HAL_OSPI_MemoryMapped(&hospi1, &sMapCfg) != HAL_OK)
  {
    return OSPI_ERROR;
  }

  return OSPI_OK;
}
